00100		TITLE SERVO
00200		.INSERT ASMBL.FAI[CMS,LCS]
00300	
00400	;Put HSTTMR in shared ram for host set?
00600	
00700	;I/O address definitions.
00800	   DAC ← 100000	;8 bit DAC.
00900	   JCR ← 120000	;Joint control output register.
01000	   ENCL ← 140000	;Encoder mux low.
01100	   ENCH ← 140001	;Encoder mux high.
01200	
01300	   STKSIZ ← 377	;Stack size.
01400	   LSBENB ← 40	;Enable LSB servo.
01500	
01600	;Zero page variables.
01700	;Not shared.
01800	
01900	DSPAT:	BLOCK 2	;Dispatch address for commands.
02000	DEFCMD:	0	;Deferred command.
02100		0	;For 16 bit even addresses.
02200	SAVPOS:	BLOCK 2	;Position for deferred servo command.
02300	
02400	CMDVEL:	BLOCK 2	;Commanded velocity.
02500	CURVEL:	BLOCK 2	;Current velocity.
02600		0	;SETPT-1.
02700	SETPT:	BLOCK 2	;Current setpoint.
02800		0	;SETINC-1.
02900	SETINC:	BLOCK 2	;Interpolating increment for setpoints.
03000		0	;OLDINC-1.
03100	OLDINC:	BLOCK 2	;Last increment, for IVEL.
03200	OLDSP:	BLOCK 2	;Last commanded setpoint, for CMDVEL.
03300	POSERR:	BLOCK 2	;Current position error.
03400	DACSIG:	BLOCK 2	;Scratch.
03500	
03600	INCTR:	0	;Count the interpolations.
03700	HSTTMR:	0	;Count ticks between host commands.
03800	
03900	LOGTMP:	BLOCK 4	;Temp for the arithmetic routines.
04000	
04100	IVEL:	BLOCK 2	;Interpolated velocity.
04200	VELERR:	BLOCK 2	;Velocity error term.
04300	VSUM:	BLOCK 2	;Sum of last 8 velocitys.
04400	VPTR:	0	;Velocity averaging index.
04500		0
04600	VELTBL:	BLOCK 20;Velocity averaging table.
04700	
04800	ZAPEND ← .-1	;Clear all the above in startup.
04900	
05000	TL:	0	;Scratch for gray to binary.
05100	TH:	0
05200	
05300	FTMP:	BLOCK 2	;Copy of friction coefficient for multiply.
     

00100	;Shared ram.
00200	   LOC 200	;Second half of zero page.
00300	
00400	;STATUS byte bits.
00500	;	7	6	5	4	3	2	1	0
00600	;            check    time      no
00700	;             word     out    tick
00800	
00900		0	;Locked.
01000	STATUS:	0	;Flags for the host.
01100	
01200	;MODE byte bits.
01300	;Bit	7	6	5	4	3	2	1	0
01400	;    servo   integ     lsb
01500	;     enlb    enlb    enbl
01600	
01700		0	;Locked.
01800	MODE:	0	;Mode bits from host.
01900	
02000	CKWORD:	BLOCK 2	;Host I/O check/command word.
02100	CMDPOS:	BLOCK 2	;Commanded position from host.
02200	
02300	;IOCTRL byte bits.
02400	;Bit	7	6	5	4	3	2	1	0
02500	;      in		      lsb   integ     pos
02600	;     tol		     enlb  disabl    mode
02700	
02800		0	;Locked.
02900	IOCTRL:	0	;Copy of JCR output port.
03000	
03100	CURPOS:	BLOCK 2	;Current position.
03200	
03300	;NINTER = function of INTSCL?
03400		0	;Locked.
03500	NINTER:	0	;# of interpolations between position
03600			;commands.
03700		0	;Locked.
03800	INTSCL:	0	;# of bits to shift setpoint dif for
03900			;interpolating.
04000		0	;Locked.
04100	HSTLIM:	0	;# of clock ticks allowed between host
04200			;commands.
04300	FRICTN:	BLOCK 2	;Viscous damping coefficient.
04400	GRAVTY:	BLOCK 2	;DC offset for gravity.
04500	POSTOL:	BLOCK 2	;Half-width of position tolerance band.
04600	INTTOL:	BLOCK 2	;Half-width of integration band.
04700	
04800	
04900	;Start of prom.
05000	   LOC 174000
05100	
05200	INITBL:	STATUS	↔	0
05400	
05500		NINTER	↔	=32
05600		INTSCL	↔	5
05700	
05800		HSTLIM	↔	=48
05900	
06000		377	;End of INITBL flag.
     

00100	;Power on reset.
00200	START:	CLD
00300		LDXI	STKSIZ	;Setup stack.
00400		TXS
00500	
00600		LDAI	0
00700		LDXI	ZAPEND
00800	RLOOP:	STAZX	0	;Reset ram.
00900		DEX
01000		BPL	RLOOP
01100		STA	DAC	;Clear DAC.
01200	
01300		LDXI	370	;-8.
01400	ZSR:	STAZX	FRICTN+10	;Clear shared ram.
01500		INX
01600		BMI	ZSR
01700	
01800		TAY
01900		BEQ	RSTDEF	;Jump
02000	
02100	
02200	DLOOP:	INY
02300		LDAY	INITBL	;Init ram.
02400		STAZX	0
02500		INY
02600	
02700	RSTDEF:	LDXY	INITBL
02800		CPXI	377
02900		BNE	DLOOP
03000	
03100	STOP:	SEI	;Go into stop mode.
03150		LDAI	0
03175		STAZ	MODE	;Clear position servo enable, etc..
03200		JSR	GETPOS	;Read encoder and convert to binary.
03300	
03400	;Sets the current position to the converted encoder value, the
03500	;setpoint the same, clears the setpoint interpolating increment,
03600	;and goes into stop mode.
03700		STAZ	CURPOS	;Set the current position.
03800		STXZ	CURPOS+1;Unlock.
03900	
04000		STAZ	SETPT	;Set the setpoint.
04100		STXZ	SETPT+1
04200		STAZ	OLDSP	;For CMDVEL.
04300		STXZ	OLDSP+1
04400	
04500		LDAI	75	;I/O control bits for servo enable on,
04600		STAZ	IOCTRL	;all others off.
04700		STA	JCR
04800	
04900		LDAI	0
05000		STAZ	SETPT-1	;Clear the setpoint extension,
05100		STAZ	SETINC-1;the interpolator,
05200		STAZ	SETINC
05300		STAZ	SETINC+1
05400		STAZ	CMDVEL	;and the commanded velocity.
05500		STAZ	CMDVEL+1
05600	
05700		STAZ	DEFCMD	;Clear the deferred command flag.
05800	
05900		CLI	;End of reset.
     

00100	RSTCKW:	LDAI	377	;Reset check word.
00200		LDXI	0
00300		SEI
00400		STAZ	CKWORD	;Lock.
00500		STXZ	CKWORD+1;Unlock.
00600		CLI
00700	;Idle loop. Wait for command.
00800	IDLE:	LDAZ	CKWORD+1;Check for new check word.
00900		BEQ	IDLE	;Not equal if bit 7 is complement of low byte.
01000	
01100		SEC
01200		SEI
01300		ADCZ	CKWORD	;Lock.
01400		LDXZ	CKWORD+1;Unlock.
01500		CLI
01600		TAY
01700		BNE	CKWDER	;Check word error.
01800	   ;Check here for immediate or deferred.
01900		TXA	;Check for valid command.
02100		ORAI	3	;3 for two commands and bit 0 = 0.
02200		ADCI	0	;Carry = 1.
02300		BNE	CKWDER	;Not a valid command.
02400	
02500		LDAZ	DEFCMD	;Check if no TICK?
02600		BNE	NOTICK	;No response since last deferred command.
02700	
02800		SEI
02900		LDYZ	CMDPOS	;Read position for servo command.
03000		LDAZ	CMDPOS+1;Unlock.
03100		CLI
03200	
03300		STYZ	SAVPOS	;Save it for later.
03400		STAZ	SAVPOS+1
03420		ASLA	;Check for valid position.
03440		BCS	CSET
03460		BMI	CKWDER
03480		BPL	GOODP
03500	CSET:	BPL	CKWDER
03520	
03600	GOODP:	STXZ	DEFCMD	;Save deferred command pointer.
03700	
03800		JMP	RSTCKW	;Handshake with host via CKWORD.
03900	
04000	CKWDER:	LDAI	100	;Set check word error flag.
04100	WSTAT:	ORAZ	STATUS
04200		STAZ	STATUS
04300		JMP	STOP
04400	
04500	;If status is in low byte. For host lockout.
04600	;	LDAI	100
04700	;	SEI
04800	;	ORAZ	STATUS
04900	;	STAZ	STATUS
05000	;	LDAZ	STATUS+1
05100	;	CLI
05200	;	JMP	STOP
05300	
05400	NOTICK:	LDAI	20	;Set tick error flag.
05600		BNE	WSTAT	;Jump.
     

00100	;Clock tick interrupt.
00200	TICK:	PHA	;Save state.
00300		TXA
00400		PHA
00500		TYA
00600		PHA
00700	
00800		JSR	GETPOS	;Read position and convert to binary.
00900	
01000		SEC
01100		SBCZ	CURPOS	;Subtract the old position
01200		STAZ	CURVEL	;yielding the velocity.
01300		TXA	;High byte of binary position.
01400		SBCZ	CURPOS+1;Unlock.
01500		STAZ	CURVEL
01600	
01700		STYZ	CURPOS	;Update the current position.
01800		STXZ	CURPOS+1;Unlock.
01900		DECZ	HSTTMR	;Count the ticks since the last command
02000		BPL	HOSTOK	;and check for timeout.
02100	
02200		LDAI	0	;Host dead. Stop.
02300		STAZ	HSTTMR
02400		STAZ	CMDVEL
02500		STAZ	CMDVEL+1
02700		LDAI	40	;Set host time out flag
02800		ORAZ	STATUS
02900		STAZ	STATUS
03000	
03100	HOSTOK:	LDAI	4
03200		BITZ	IOCTRL	;If position mode is off,
03300		BNE	INTVEL
03400		JMP	CURSRV	;don't servo.
03500	
03600	INTVEL:	CLC	;Interpolate the velocity.
03700		LDAZ	SETINC-1
03800		ADCZ	OLDINC-1;IVEL ← OLDINC + SETINC.
03900		STAZ	OLDINC-1
04000		LDAZ	SETINC
04100		ADCZ	OLDINC
04200		STAZ	IVEL
04300		LDAZ	SETINC+1
04400		ADCZ	OLDINC+1
04500	
04600		ASLZ	OLDINC-1;IVEL ← IVEL * 4
04700		ROLZ	IVEL
04800		ROLA
04900		ASLZ	OLDINC-1
05000		ROLZ	IVEL
05100		ROLA
05200		STAZ	IVEL+1
05300	
05400		LDAZ	SETINC-1;OLDINC ← SETINC.
05500		STAZ	OLDINC-1
05600		LDAZ	SETINC
05700		STAZ	OLDINC
05800		LDAZ	SETINC+1
05900		STAZ	OLDINC+1
     

00100	;Interpolate the setpoints.
00200	INTRS:	CLC
00300		LDAZ	SETPT-1
00400		ADCZ	SETINC-1;Add the increment to the setpoint.
00500		STAZ	SETPT-1
00600		LDAZ	SETPT
00700		ADCZ	SETINC
00800		STAZ	SETPT
00900		LDAZ	SETPT+1
01000		ADCZ	SETINC+1
01100		STAZ	SETPT+1
01200	
01300		DECZ	INCTR	;Check if this is the last interpolation.
01400		BNE	GPOSER
01500	
01600		LDAI	0	;Clear SETINC if done interpolating.
01700		STAZ	SETINC-1
01800		STAZ	SETINC
01900		STAZ	SETINC+1
02000	
02100	;Calculate the position error.
02200	GPOSER:	SEC
02300		LDAZ	CURPOS	;POSERR ← CURPOS - SETPT.
02400		SBCZ	SETPT
02500		STAZ	POSERR
02600		LDAZ	CURPOS+1
02700		SBCZ	SETPT+1
02800		STAZ	POSERR+1
     

00100		BITZ	MODE	;If servo is disabled, we're
00200		BPL	OOTOL	;automatically out of tolerance
00300	
00400		LDAZ	POSERR+1;Test the sign of pos error.
00500		BMI	NEGPER
00600	
00700		LDAZ	POSTOL	;Positive. Compare with tol.
00800		CMPZ	POSERR
00900		LDAZ	POSTOL+1;Unlock.
01000		SBCZ	POSERR+1
01100		BCS	TOLOK	;In tolerance.
01200		BCC	OOTOL	;Jump.
01300	
01400	NEGPER:	CLC	;Negative. Add the tolerance.
01500		LDAZ	POSTOL	;Lock.
01600		ADCZ	POSERR
01700		LDAZ	POSTOL+1;Unlock.
01800		ADCZ	POSERR+1
01900		BCS	TOLOK	;In tolerance.
02000	
02100	OOTOL:	LDAZ	IOCTRL	;Out of tolerance.
02200		ANDI	177	;Turn off the in tolerance
02300		BNE	WCNTRL	;indicator. Jump.
02400	
02500	TOLOK:	LDAZ	IOCTRL	;In tolerance. Turn it on.
02600		ORAI	200
02700	WCNTRL:	STAZ	IOCTRL
02800		STA	JCR	;Copy it to output.
02900	
03000		BITZ	MODE	;If intergration is disabled,
03100		BVC	OOBAND	;turn it off.
03200		LDAZ	POSERR+1;Test sign of position error.
03300		BMI	ADTOL
03400	
03500		LDAZ	INTTOL	;Positive. Compare with tol.
03600		CMPZ	POSERR
03700		LDAZ	INTTOL+1;Unlock.
03800		SBCZ	POSERR+1
03900		BCS	INBAND	;In band. Turn on integrator.
04000		BCC	OOBAND	;Jump.
04100	
04200	ADTOL:	CLC	;Negative. Add the tolerance.
04300		LDAZ	INTTOL	;Lock.
04400		ADCZ	POSERR
04500		LDAZ	INTTOL+1;Unlock.
04600		ADCZ	POSERR+1
04700		BCS	INBAND	;Check if in band.
04800	
04900	OOBAND:	LDAZ	IOCTRL	;Out of band. Turn off
05000		ORAI	10	;integration by setting the
05100		ANDI	357	;control bit. LSB servo off.
05200		BNE	WCTRL2	;Jump.
     

00100	INBAND:	LDAI	LSBENB	;In band. Is LSB servo enabled?
00200		BITZ	MODE
00300		BEQ	RCNTRL
00400	
00500		LDAZ	POSERR	;Yes. Is the error exactly 0?
00600		ORAZ	POSERR+1
00700		BNE	RCNTRL
00800	
00900		LDAZ	IOCTRL	;It is. Integration off, LSB
01000		ORAI	30	;servo on.
01100		BNE	WCTRL2	;Jump.
01200	
01300	RCNTRL:	LDAZ	IOCTRL	;LSB disabled or error
01400		ANDI	347	;not zero. LSB servo off,
01500				;integration on.
01600	
01700	WCTRL2:	STAZ	IOCTRL
01800		STA	JCR	;Output it.
01900	
02000	;Get the velocity error.
02100		CLC
02200		LDAZ	VSUM
02300		ADCZ	CURVEL	;VSUM ← VSUM + CURVEL.
02400		TAX
02500		LDAZ	VSUM+1
02600		ADCZ	CURVEL+1
02700		TAY
02800		TXA
02900		LDXZ	VPTR	;Get velocity averaging index.
03000		SEC
03100		SBCZX	VELTBL	;VSUM ← VSUM - VELTBL[VPTR].
03200		STAZ	VSUM
03300		TYA
03400		SBCZX	VELTBL+10
03500		STAZ	VSUM+1
03600		TAY
03700	
03800		LDAZ	CURVEL	;VELTBL[VPTR] ← CURVEL.
03900		STAZX	VELTBL
04000		LDAZ	CURVEL+1
04100		STAZX	VELTBL+10
04200		INX	;VPTR ← (VPTR + 1) .AND. (VTLEN - 1).
04300		TXA
04400		ANDI	7
04500		STAZ	VPTR
04600	
04700		SEC
04800		LDAZ	VSUM
04900		SBCZ	IVEL	;VELERR ← VSUM - IVEL.
05000		STAZ	VELERR
05100		TYA	;A ← VSUM+1.
05200		SBCZ	IVEL+1
05300	
05400		LDXZ	3	;Number of right shifts for divide by 8.
05500	VRSCL:	CMPI	200	;Extend sign.
05600		RORA	;A = VELERR+1.
05700		RORZ	VELERR	;VELERR ← VELERR / 8.
05800		DEX
05900		BNE	VRSCL
     

00100		LDYZ	VELERR	;Get the velocity error,
00200			;A = VELERR+1
00300		JSR	LOG
00400		LDXZ	FRICTN	;(Copy friction for multiply.)
00500		STXZ	FTMP
00600		LDXZ	FRICTN+1;Unlock.
00700		STXZ	FTMP+1
00800		LDXI	FTMP	;multiply by the friction
00900		JSR	MULTIP	;coefficient,
01000		JSR	EXP
01100	
01200		TAX	;Save high byte.
01300		TYA	;Get low byte.
01400		CLC	;add the position error...
01500		ADCZ	POSERR
01600		STAZ	DACSIG
01700		TXA
01800		ADCZ	POSERR+1
01900		STAZ	DACSIG+1
02000	
02100		CLC	;...and the gravity offset.
02200		LDAZ	DACSIG
02300		ADCZ	GRAVTY	;Lock.
02400		TAY	;Save low byte.
02500		LDAZ	GRAVTY+1;Unlock.
02600		ADCZ	DACSIG+1
02700	
02800		JSR	PUTDAC	;Put result out to the DAC.
02900	
03000	CMDSP:	LDAZ	DEFCMD	;Check for a command.
03100		BEQ	INTXIT
03200		ANDI	2	;Low nibble command bit.
03300		TAX
03400		LDAX	CMDTBL	;Get command address.
03500		STAZ	DSPAT
03600		LDAX	CMDTBL+1
03700		STAZ	DSPAT+1
03800		JMPIN	DSPAT	;Execute command.
03900	
04000	CMDEND:	LDAI	0	;Done with deferred command.
04100		STAZ	DEFCMD	;Reset command word.
04200	INTXIT:	PLA	;Restore state and dismiss interrupt.
04300		TAY
04400		PLA
04500		TAX
04600		PLA
04700		RTI
04800	
04900	CURSRV:	LDAI	0	;Not servoing ("Current mode")...
05000		STAZ	SETPT-1	;Make the setpoint track
05100		LDAZ	CURPOS	;the current position in order to
05200		STAZ	SETPT	;keep the arm from twitching when
05300		LDAZ	CURPOS+1;the host enables the servo. Unlock.
05400		STAZ	SETPT+1
05500		JMP	CMDSP	;Go check on commands.
05600	
05700	CMDTBL:		;DEFERRED COMMAND TABLE.
05800		CMDEND∧377	;Nop.
05900		(CMDEND⊗-10)∧377
06000		CMDSRV∧377	;Servo command.
06100		(CMDSRV⊗-10)∧377
     

00100	;Deferred commands.
00200	CMDSRV:	LDAZ	MODE	;Servo command.
00300		ANDI	202	;Test for servo enabled.
00400		CMPI	200
00500		BEQ	ENBLD
00600		JMP	CMDEND	;No. End this command. 
00700	
00800	ENBLD:	LDAZ	SAVPOS	;Enabled.
00900		LDXZ	SAVPOS+1;Get commanded position.
01200		SEC
01300		SBCZ	SETPT	;Get differance between next position
01400		STAZ	SETINC	;and the last setpoint.
01500		TXA
01600		SBCZ	SETPT+1
01700		LDXI	0
01800		STXZ	SETPT-1	;Clear setpoint and increment extentions.
01900		STXZ	SETINC-1
02000		LDXZ	INTSCL
02100	
02200	SCAL:	CMPI	200	;Extend sign.
02300		RORA	;Divide the differance by the number of interpolations.
02400		RORZ	SETINC
02500		RORZ	SETINC-1
02600		DEX
02700		BNE	SCAL
02800	
02900		STAZ	SETINC+1;Which yields the interpolating increment.
03000		LDAZ	NINTER
03100		STAZ	INCTR	;Setup the interpolator count.
03200		SEC
03300		LDAZ	SAVPOS
03400		SBCZ	OLDSP
03500		STAZ	CMDVEL	;CMDVEL ← CMDPOS - OLDSP.
03600		LDAZ	SAVPOS+1
03700		SBCZ	OLDSP+1
03800		STAZ	CMDVEL+1
03900		LDAZ	SAVPOS
04000		STAZ	OLDSP	;OLDSP ← CMDPOS.
04100		LDAZ	SAVPOS+1
04200		STAZ	OLDSP+1
04300	
04400		LDAZ	IOCTRL
04500		ORAI	44	;Turn on servo and current mode enable bits.
04600		STAZ	IOCTRL
04700		STA	JCR	;Output it.
04800		LDAZ	HSTLIM	;Reset host timer.
04900		STAZ	HSTTMR
05000		JMP	CMDEND
     

00100	;Position conversion routine.
00200	GETPOS:	LDY	ENCL	;Read encoder.
00300		LDA	ENCH
00400		EORI	377	;Complement it.
00500	;Convert from gray to binary.
00600		STAZ	TH
00700		LSRA	;Shift by 1.
00800		EORZ	TH
00900		STAZ	TH
01000		TAX	;X ← high byte.
01100	
01200		TYA
01300		EORI	377	;Complement low byte.
01400		STAZ	TL
01500		RORA
01600		EORZ	TL
01700		STAZ	TL
01800	
01900		LSRZ	TH	;Shift by 2.
02000		RORA
02100		LSRZ	TH
02200		RORA
02300		EORZ	TL
02400		STAZ	TL
02500		TAY	;Y ← low byte.
02600	
02700		TXA	;Get high byte.
02800		EORZ	TH
02900		STAZ	TH
03000	
03100		LSRA	;Shift by 4.
03200		RORZ	TL
03300		LSRA
03400		RORZ	TL
03500		LSRA
03600		RORZ	TL
03700		LSRA
03800		RORZ	TL
03900	
04000		EORZ	TH
04100		STAZ	TH
04200		TYA
04300		EORZ	TL
04400		EORZ	TH	;Shift by 8.
04500		TAY	;Save low byte.
04600	
04700		LDXZ	TH	;Get high byte.
04800		BITZ	TH
04900		BVC	POS	;Check if negative.
05000		TXA
05100		ORAI	200	;Extend sign.
05200		TAX
05300	
05400	POS:	TYA	;Returns with position in A, Y (low) and X (high).
05500		RTS
     

00100	;DAC output subroutine.
00200	;Enter with 2 byte value in Y (low), A (high).
00300	;Clobbers all registers, but the 8 bits the DAC got are returned in A.
00400	PUTDAC:	BMI	NEGDAC	;Assuming the last inst. loaded A.
00500		CPYI	200	;Positive. Compare with 2↑7.
00600		SBCI	0
00700		BCC	INRNGE
00800	
00900	TOOHI:	LDYI	177	;Too high. Saturate positive.
01000		BNE	INRNGE	;Jump.
01100	
01200	NEGDAC:	CPYI	200	;Negative. Compare with -2↑7.
01300		SBCI	377
01400		BCS	INRNGE
01500	
01600	TOOLOW:	LDYI	200	;Too low. Saturate to -2↑7.
01700	
01800	INRNGE:	LDAY	VETBL	;Straighting it.
01900		STA	DAC	;Output 8 bits to the DAC.
02000		RTS
     

00100	;Arithmetic routines.
00200	;Enter with high byte in A, low in Y.
00300	;Returns A = characteristic and sign, Y = mantissa.
00400	;Clobbers X, LOGTMP, LOGTMP+1.
00500	LOG:	STYZ	LOGTMP	;Save the inputs.
00600		STAZ	LOGTMP+1
00700	
00800		LDXI	20+100	;Init characteristic to 15.
00900		CMPI	0	;Test sign of input.
01000		BPL	POSIN
01100		SEC	;Negative. 2's complement it.
01200		LDAI	0
01300		SBCZ	LOGTMP
01400		STAZ	LOGTMP
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIN:	BNE	NORML	;Is high byte zero?
01800		LDAZ	LOGTMP	;Yes. Low byte?
01900		BEQ	RTRN	;If so, return zero.
02000		LDYI	0	;Low nonzero. Shift left one
02100		STYZ	LOGTMP	;byte,
02200		LDXI	10+100	;change characteristic to 7.
02300	NORML:	DEX	;Normalize the number, counting the
02400		ASLZ	LOGTMP	;characteristic down. When the
02500		ROLA	;first "1" shifts out, we've subtracted
02600		BCC	NORML	;1 from the normalized number
02700		ASLZ	LOGTMP	;(This rounds the result)
02800		ADCI	=11	;and are left with the fraction
02900		TAY	;Adding 11 to that is equivalent to
03000		TXA	;adding 0.043.
03100		ADCI	0	;Propagate the carry into the
03200				;characteristic.
03300		ASLA	;Insert the sign bit from the saved
03400		ASLZ	LOGTMP+1;input.
03500		RORA
03600	RTRN:	RTS	;Done.
03700	
03800	;Enter with sign and characteristic in A, mantissa in Y
03900	;Returns 16-bit integer, low byte in Y, high in A.
04000	;Clobbers X, LOGTMP, LOGTMP+1.
04100	EXP:	STAZ	LOGTMP+1;Save sign of input.
04200		ANDI	177	;Mask it off.
04300		BEQ	ZEROIN	;Zero characteristic returns
04400		TAX	;zero.
04500		TYA	;Get the mantissa...
04600		SEC
04700		SBCI	=11	;...subtract 0.043...
04800		STAZ	LOGTMP	;(save this value)
04900		TXA	;...propagate the carry and get rid
05000		SBCI	100	;of the XS-64 offset.
05100		BMI	NEGIN	;If negative (value < 1.0)
05200				;return zero.
05300		CMPI	=15	;Test for overflow (value>=2↑15
05400		BCS	SATUR
05500		TAX	;...no. Number is in range.
05600		ADCI	370	;Is characteristic below 8?
05700		BMI	BLOATE
05800		TAX	;No. Reduce if by 8,
05900		JSR	UNNORM	;unnormalize.
06000		BMI	GETTMP	;Jump.
     

00100	BLOATE:	JSR	UNNORM	;Yes. Unnormalize, then
00200		ASLZ	LOGTMP	;(round result)
00300		ADCI	0
00400		STAZ	LOGTMP	;use result as low byte and
00500		LDAI	0	;set high byte to zero.
00600	
00700	GETTMP:	LDYZ	LOGTMP
00800	GTMP1:	LDXZ	LOGTMP+1;Test sign of input...
00900		BPL	POSIGN
01000		STAZ	LOGTMP+1;...negative. 2's complement
01100		LDAI	0	;the result.
01200		SEC
01300		SBCZ	LOGTMP
01400		TAY
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIGN:	RTS
01800	
01900	NEGIN:	LDAI	0	;Set the result to zero if the
02000	ZEROIN:	TAY	;input is negative.
02100		RTS
02200	
02300	SATUR:	LDYI	377	;Saturate result to 2↑15 - 1 if
02400		STYZ	LOGTMP	;input was 15 or more.
02500		LDAI	177
02600		BNE	GTMP1	;Jump.
02700	
02800	UNNORM:	LDAI	1	;Unnormalize subroutine. Add 1
02900		BNE	DECRX	;to the fraction. Jump.
03000	
03100	SCALE:	ASLZ	LOGTMP	;Scale the fraction left by the
03200		ROLA	;amount of the characteristic.
03300	DECRX:	DEX
03400		BPL	SCALE
03500		RTS
03600	
03700	;Enter with characteristic of multiplier in A,
03800	;mantissa in Y, X pointing to a pair of base page
03900	;locations containing the multiplicand (mantissa in the
04000	;low byte).
04100	;Returns the product in A and Y, same form as the
04200	;multiplier. Leaves X unchanged. Clobbers LOGTMP and
04300	;LOGTMP+1.
04400	MULTIP:	PHA
04500		EORZX	1	;Compute sign of result,
04600		STAZ	LOGTMP+1	;save it away.
04700		PLA
04800		ANDI	177	;Mask off multiplier sign.
04900		BEQ	ZEROIN	;If zero, return zero.
05000		STAZ	LOGTMP
05100		TYA	;Add the two logarithms.
05200		CLC
05300		ADCZX	0
05400		TAY
05500		LDAZX	1
05600		ANDI	177	;If multiplicand is zero,
05700		BEQ	ZEROIN	;return a zero.
05800		ADCZ	LOGTMP
05900		SEC
06000		SBCI	100	;Correct the XS-64 offset.
     

00100		BPL	INSIGN	;Result in range?
00200		ANDI	100	;No. If underflow,
00300		BNE	NEGIN	;return zero.
00400		LDAI	177	;Overflow. Saturate to
00500		LDYI	377	;highest magnitude.
00600	
00700	INSIGN:	ASLA	;Insert the sign of the result.
00800		ASLZ	LOGTMP+1
00900		RORA
01000		RTS
01100	
01200	;Inverse function: 2's complement the magnitude part
01300	;of a 15-bit logarithm.
01400	;Enter with characteristic in A, mantissa in Y.
01500	;Returns inverse in the same form. X unchanged.
01600	;Clobbers LOGTMP and LOGTMP+1.
01700	INV:	STYZ	LOGTMP	;Pretty straightforward...
01800		STAZ	LOGTMP+1
01900		SEC
02000		LDAI	0	;Complement the number by
02100		SBCZ	LOGTMP	;subtracting it from zero.
02200		TAY
02300		LDAI	0
02400		SBCZ	LOGTMP+1
02500		JMP	INSIGN	;Insert the original sign.
02600	
02700	;DAC output table.
02800	   LOC (.∨377)+1	;For start of next page.
02900	VETBL:		;DAC output table.
03000	N ← 0
03100	REPEATE 400,{N ↔ N←N+1 ↔}
03200	
03300	   NMI ← START	;Reset??
03400	;Interrupt vectors.
03500	   LOC 177772
03600		NMI∧377
03700		(NMI⊗-10)∧377
03800		START∧377
03900		(START⊗-10)∧377
04000		TICK∧377
04100		(TICK⊗-10)∧377
04200	END